#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy, sys, os
import moveit_commander
from moveit_msgs.msg import RobotTrajectory
from actionlib_msgs.msg import *
from trajectory_msgs.msg import JointTrajectoryPoint
from geometry_msgs.msg import PoseStamped, Pose,  Point, Quaternion
from vision_msgs.msg import BoundingBox2D, Detection2DArray,ObjectHypothesisWithPose,Detection2D
from nav import *



class MoveItNavGrasp:
    def __init__(self):
        # 初始化ROS节点
        rospy.init_node('moveit_grasp_demo')
        r = rospy.Rate(10)
        rospy.on_shutdown(self.shutdown)

        self.obj_grasp_pose =  rospy.get_param("~obj_grasp_pose", '3.552, -1.372, 0.000')
        self.obj_grasp_quat =  rospy.get_param("~obj_grasp_quat", '0.000, 0.000, 1.000, 0.010')
        self.obj_place_pose = rospy.get_param("~obj_place_pose", '-0.049,  -0.853, 0')
        self.obj_place_quat = rospy.get_param("~obj_place_quat", '0.000, 0.000, -0.024, 1.000')

                
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv) 
        # 初始化需要使用move group控制的机械臂中的arm group
        self.arm = moveit_commander.MoveGroupCommander('manipulator')        
        # 获取终端link的名称
        self.end_effector_link = self.arm.get_end_effector_link()
        # 设置目标位置所使用的参考坐标系
        self.reference_frame = 'lebai_base_link'
        self.arm.set_pose_reference_frame(self.reference_frame)
        # 当运动规划失败后，允许重新规划
        self.arm.allow_replanning(True)
        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
        self.arm.set_goal_position_tolerance(0.01)
        self.arm.set_goal_orientation_tolerance(0.05)


        self.cnt = 0
        self.stable_cnt = 4
        self.nav_step = 0
        self.grasp_status = False
        self.goal_lock = False
        self.grasp_finish = False

        self.gripper_camera_x = 38
        self.gripper_camera_y = 80
        self.gripper_camera_z = 175
        self.grasp_obj = []

        self.object_sub = rospy.Subscriber("/object_point_info", Detection2D, self.ObjectCallback)

        self.home_pose()
        rospy.sleep(1)
        self.gripper_open()

        grasp_pose = list(map(float, self.obj_grasp_pose.split(',')))
        grasp_quat = list(map(float, self.obj_grasp_quat.split(',')))
        place_pose = list(map(float, self.obj_place_pose.split(',')))
        place_quat = list(map(float, self.obj_place_quat.split(',')))
        self.GraspGoal = Pose(Point(grasp_pose[0], grasp_pose[1], grasp_pose[2]), Quaternion(grasp_quat[0], grasp_quat[1], grasp_quat[2], grasp_quat[3]))
        self.PlaceGoal = Pose(Point(place_pose[0], place_pose[1], place_pose[2]), Quaternion(place_quat[0], place_quat[1], place_quat[2], place_quat[3]))

        self.nav = RikibotNav()


        while not rospy.is_shutdown():
            if  self.goal_lock is True:
                self.init_pose()
                rospy.sleep(1)
                if self.grasp_status is True:
                    self.cnt = 0
                    self.grasp_ctrl()
            else:
                self.MoveSingleGrasp()

            r.sleep()


    def ObjectCallback(self, msg):
        obj_info = msg
        self.cnt = self.cnt + 1
        if self.cnt > self.stable_cnt and self.grasp_status is False:
            self.grasp_status = True
            self.grasp_obj = obj_info.results[-1].pose.pose.position
            #print(self.grasp_obj)
       
    def camera_to_gripper_pose(self, x, y, z):
        print(x, y, z)
        self.arm_pose = self.arm.get_current_pose()
        print(self.arm_pose.pose.position.x, self.arm_pose.pose.position.y, self.arm_pose.pose.position.z)
        grasp_x = ((round(self.arm_pose.pose.position.x*1000.00, 2) - x) + self.gripper_camera_x)/1000.00
        grasp_y = ((round(self.arm_pose.pose.position.y*1000.00, 2) + y) - self.gripper_camera_y)/1000.00
        grasp_z = self.arm_pose.pose.position.z + float(self.gripper_camera_z - z)/1000.00
        return grasp_x, grasp_y, grasp_z

    def grasp_ctrl(self):
        rospy.loginfo("start grasp object")
        x, y, z = self.camera_to_gripper_pose(self.grasp_obj.x, self.grasp_obj.y, self.grasp_obj.z)
        print(x, y, z)
        self.move_pose(x, y, z)
        rospy.sleep(1)
        self.gripper_close()
        rospy.sleep(1)
        self.init_pose()
        rospy.sleep(1)
        self.home_pose()
        rospy.sleep(1)
        self.cnt = 0
        self.grasp_status = False
        self.goal_lock  = False
        self.grasp_finish = True

    def place_ctrl(self):
        rospy.loginfo("start place object")
        self.init_pose()
        rospy.sleep(1)
        self.gripper_open()
        rospy.sleep(1)
        self.home_pose()


    def move_pose(self, x, y, z):
        target_pose = PoseStamped()
        target_pose.header.frame_id = self.reference_frame
        target_pose.header.stamp = rospy.Time.now()     
        target_pose.pose.position.x = x
        target_pose.pose.position.y = y
        target_pose.pose.position.z = z
        target_pose.pose.orientation = self.arm.get_current_pose().pose.orientation
        # 设置机器臂当前的状态作为运动初始状态
        self.arm.set_start_state_to_current_state()
        
        # 设置机械臂终端运动的目标位姿
        self.arm.set_pose_target(target_pose, self.end_effector_link)
        
        # 规划运动路径
        traj = self.arm.plan()
        
        # 按照规划的运动路径控制机械臂运动
        self.arm.execute(traj)


    def init_pose(self):
        # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
        # 姿态使用四元数描述，基于base_link坐标系
        target_pose = PoseStamped()
        target_pose.header.frame_id = self.reference_frame
        target_pose.header.stamp = rospy.Time.now()     
        target_pose.pose.position.x = -0.1126
        target_pose.pose.position.y = -0.4000
        target_pose.pose.position.z = -0.1868
        target_pose.pose.orientation.x = 0.00244851260251
        target_pose.pose.orientation.y = -0.00192698956972
        target_pose.pose.orientation.z = -0.706303928748
        target_pose.pose.orientation.w = 0.707901865891 
        
        # 设置机器臂当前的状态作为运动初始状态
        self.arm.set_start_state_to_current_state()
        
        # 设置机械臂终端运动的目标位姿
        self.arm.set_pose_target(target_pose, self.end_effector_link)
        
        # 规划运动路径
        traj = self.arm.plan()
        
        # 按照规划的运动路径控制机械臂运动
        self.arm.execute(traj)

    def home_pose(self):
        # 控制机械臂回到初始化位置
        self.arm.set_named_target('look')
        self.arm.go()
    
    def gripper_open(self):
        os.system("rosservice call /io_service/set_gripper_position \"{val: 90}\"")
        rospy.sleep(1)

    def gripper_close(self):
        os.system("rosservice call /io_service/set_gripper_position \"{val: 10}\"")
        rospy.sleep(1)

    def MoveSingleGrasp(self):
        if self.nav_step == 0:
            self.nav.SendGoal(self.GraspGoal)
        if self.nav_step == 1 and self.grasp_finish is True:
            self.nav.SendGoal(self.PlaceGoal)
        if self.nav_step == 2:
            self.place_ctrl()

        state =self.nav.NavState()
        if state == 1:
            self.nav_step = self.nav_step + 1
            if self.nav_step == 1:
                self.goal_lock  = True


    def shutdown(self):
        # Release handle to the webcam
        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
        rospy.logwarn("now will shutdown grasp_node ...")



if __name__ == "__main__":
    MoveItNavGrasp()

    
    
